#pragma once
#include "yaml-cpp/yaml.h"
#include <tf/tf.h>
#include <ros/ros.h>
#include "robot_info.h"

class cope_yaml
{
        private:
                uint8_t total_info_num;
                std::string yaml_path;
                std::vector<InfoTypedef> _info;        
                YAML::Node yaml_node;
        public:
                void loadInfo();
                void getYamlPath(const std::string _path);
                uint8_t getYamlId(std::string _name);
                geometry_msgs::Pose  getYamlPose(std::string _name);

                cope_yaml();
                ~cope_yaml();
};

void cope_yaml::loadInfo(){
        yaml_node = YAML::LoadFile(yaml_path);
        
        InfoTypedef tmp_info;
        total_info_num = yaml_node["Total"].as<int>();
        for (int i = 0; i< total_info_num; i++)
        {
                tmp_info.id = yaml_node["Points"][i]["id"].as<int>();
                tmp_info.tag_name = yaml_node["Points"][i]["tag"].as<std::string>();
                for (int j = 0; j < 3; j++)
                {
                        tmp_info.rpy[j] = yaml_node["Points"][i]["rpy"][j].as<double>();
                        tmp_info.xyz[j] = yaml_node["Points"][i]["xyz"][j].as<double>();
                }
                tmp_info.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(      
                                                                                tmp_info.rpy[0],
                                                                                tmp_info.rpy[1],
                                                                                tmp_info.rpy[2]);
                tmp_info.pose.position.x = tmp_info.xyz[0];
                tmp_info.pose.position.y = tmp_info.xyz[1];
                tmp_info.pose.position.z = tmp_info.xyz[2];
                _info.push_back(tmp_info);
        }
}
void cope_yaml::getYamlPath(const std::string _path){
        yaml_path = _path.c_str();
}

uint8_t cope_yaml::getYamlId(std::string _name){
        for (auto  i:_info){
                if(i.tag_name == _name)
                        return i.id;
        }
}

geometry_msgs::Pose  cope_yaml::getYamlPose(std::string _name){
        for (auto  i : _info){
                if (i.tag_name == _name)
                        return i.pose;
        }
}

cope_yaml::cope_yaml(){}
cope_yaml::~cope_yaml(){}

